#include "intersection.h"
#include "math.h"
#include "facing.h"
//#include "rect.h"
#include "util/mmgr.h"		// Needs to come in early

#include <math.h>
#include <float.h>
#include <cassert>

using namespace math;

// Constructors
#define CLIP_RIGHT	(1<<0)	// cohen-sutherland clipping outcodes
#define CLIP_LEFT	(1<<1)
#define CLIP_TOP	(1<<2)
#define CLIP_BOTTOM	(1<<3)
#define CLIP_FRONT	(1<<4)
#define CLIP_BACK	(1<<5)

static unsigned long calc_outcode( const Vec3& bbox_min, const Vec3& bbox_max, const Vec3& pnt )
{
	unsigned long outcode = 0;

	if( pnt.x > bbox_max.x ) {
		outcode |= CLIP_RIGHT;
	} else if( pnt.x < bbox_min.x ) {
		outcode |= CLIP_LEFT;
	}
	if( pnt.y > bbox_max.y ) {
		outcode |= CLIP_TOP;
	} else if( pnt.y < bbox_min.y ) {
		outcode |= CLIP_BOTTOM;
	}
	if( pnt.z > bbox_max.z ) {
		outcode |= CLIP_BACK;
	} else if( pnt.z < bbox_min.z ) {
		outcode |= CLIP_FRONT;
	}

	return outcode;
}

// Curtesy of tim schroeder
bool Intersection::RayAABB(const Vec3& _vMin,	const  Vec3& _vMax, const  Vec3& _vFrom, const  Vec3& _vTo, Vec3&  _vPos) 
{
	unsigned long outcode1, outcode2;

	outcode1 = calc_outcode( _vMin, _vMax, _vFrom );
	if( outcode1 == 0 ) {
		// point inside bounding box
		_vPos = _vFrom;
		return true;
	}

	outcode2 = calc_outcode( _vMin, _vMax, _vTo );
	if( outcode2 == 0 ) {
		// point inside bounding box
		_vPos = _vTo;
		return true;
	}

	if( (outcode1 & outcode2) > 0 ) {
		// both points on same side of box
		return false;
	}

	// check intersections
	if( outcode1 & (CLIP_RIGHT | CLIP_LEFT) ) {
		if( outcode1 & CLIP_RIGHT ) {
			_vPos.x = _vMax.x;
		} else {
			_vPos.x = _vMin.x;
		}
		float x1 = _vTo.x - _vFrom.x;
		float x2 = _vPos.x - _vFrom.x;
		_vPos.y = _vFrom.y + x2 * (_vTo.y - _vFrom.y) / x1;
		_vPos.z = _vFrom.z + x2 * (_vTo.z - _vFrom.z) / x1;

		if( _vPos.y <= _vMax.y && _vPos.y >= _vMin.y && _vPos.z <= _vMax.z && _vPos.z >= _vMin.z ) {
			return true;
		}
	}

	if( outcode1 & (CLIP_TOP | CLIP_BOTTOM) ) {
		if( outcode1 & CLIP_TOP ) {
			_vPos.y = _vMax.y;
		} else {
			_vPos.y = _vMin.y;
		}
		float y1 = _vTo.y - _vFrom.y;
		float y2 = _vPos.y - _vFrom.y;
		_vPos.x = _vFrom.x + y2 * (_vTo.x - _vFrom.x) / y1;
		_vPos.z = _vFrom.z + y2 * (_vTo.z - _vFrom.z) / y1;

		if( _vPos.x <= _vMax.x && _vPos.x >= _vMin.x && _vPos.z <= _vMax.z && _vPos.z >= _vMin.z ) {
			return true;
		}
	}

	if( outcode1 & (CLIP_FRONT | CLIP_BACK) ) {
		if( outcode1 & CLIP_BACK ) {
			_vPos.z = _vMax.z;
		} else {
			_vPos.z = _vMin.z;
		}
		float z1 = _vTo.z - _vFrom.z;
		float z2 = _vPos.z - _vFrom.z;
		_vPos.x = _vFrom.x + z2 * (_vTo.x - _vFrom.x) / z1;
		_vPos.y = _vFrom.y + z2 * (_vTo.y - _vFrom.y) / z1;

		if( _vPos.x <= _vMax.x && _vPos.x >= _vMin.x && _vPos.y <= _vMax.y && _vPos.y >= _vMin.y ) {
			return true;
		}
	}

	// nothing found
	return false;
}


bool	Intersection::RayTri(const Vec3& orig, const Vec3& dir, const Vec3& vert0, const Vec3& vert1, const Vec3& vert2, float* t )
{
	// find vectors for two edges sharing vert0
	Vec3 edge1 = vert1 - vert0;
	Vec3 edge2 = vert2 - vert0;

	// begin calculating determinant - also used to calculate U parameter
	Vec3 pvec = dir * edge2;

	// if determinant is near zero, ray lies in plane of triangle
	float det = edge1^ pvec ;

	// the culling branch
	if ( fabsf(det) <= FLT_MIN )
		return false;
	float invdet = 1.f / det;

	// calculate distance from vert0 to ray orig
	Vec3 tvec = orig - vert0;

	// calculate U parameter and test bounds
	float u = (tvec ^ pvec) * invdet;
	if ( u < 0.f || u > 1.f )
		return false;

	// prepare to test V parameter
	Vec3 qvec = tvec * edge1;

	// calculate V parameter and test bounds
	float v = (dir ^ qvec) * invdet;
	if ( v < 0.f || u + v > 1.f )
		return false;

	// calculate s, ray intersects triangle
	float s = (edge2 ^ qvec) * invdet;
	if ( s < 0.f )
		return false;

    if (t)
        (*t) = s;
	return true;
}

bool	Intersection::RaySphere(const Vec3& From, const Vec3& To, const Vec3& Center, float Radius, math::Vec3& Intersect)
{
	math::Vec3	Delta = To - From;
	math::Vec3	Intersection;

	float t =(  ( ( Center.x - From.x ) * Delta.x ) +
				( ( Center.y - From.y ) * Delta.y ) +
				( ( Center.z - From.z ) * Delta.z ) ) /
				( Delta.MagSqr() );
 
    if( t < 0.0f || t > 1.0f )
	{
        // closest point does not fall within the line segment
		// check this as a capsule (check distance from FROM and TO as well?
		return false;
	}
 
    Intersect.x = From.x + t * ( Delta.x );
    Intersect.y = From.y + t * ( Delta.y );
    Intersect.z = From.z + t * ( Delta.z );
 
	return (Intersect - Center).MagSqr() < (Radius*Radius);
}

bool	Intersection::RayCircle(const Vec2& From, const Vec2& To, const Vec2& Center, float Radius, math::Vec2& Intersect)
{
	math::Vec2	Delta = To - From;
	math::Vec2	Intersection;

	float t =(  ( ( Center.x - From.x ) * Delta.x ) +
				( ( Center.y - From.y ) * Delta.y )) /
				Delta.MagSqr() ;
 
    if( t < 0.0f || t > 1.0f )
	{
        // closest point does not fall within the line segment
		// check this as a capsule (check distance from FROM and TO as well?
		return false;
	}
 
    Intersect.x = From.x + t * ( Delta.x );
    Intersect.y = From.y + t * ( Delta.y );
 
	return (Intersect - Center).MagSqr() < (Radius*Radius);
}

bool	Intersection::AABB2D(const math::Vec2& MinA, const math::Vec2& MaxA, const math::Vec2& MinB, const math::Vec2& MaxB)
{

	math::Vec2		CenterA = MinA + ((MaxA - MinA) * 0.5f);
	math::Vec2		CenterB = MinB + ((MaxB - MinB) * 0.5f);
	math::Vec2		Delta = CenterB-CenterA;
	return Delta.x < ((MaxA.x - MinA.x) * 0.5f) + ((MaxB.x - MinB.x) * 0.5f) && 
			Delta.y < ((MaxA.y - MinA.y) * 0.5f) + ((MaxB.y - MinB.y) * 0.5f);
}

bool Intersection::SphereCapsule2D(const math::Vec2& Center, float Radius, const math::Vec2& From, const math::Vec2& To, float LineRad, float& Penetration, math::Vec2& Normal)
{
	// find the closest point
	math::Vec2	Delta = To - From;
	if (Delta.Magnitude() < 0.000001f) return false;	// zero length vector!

	math::Vec2	Intersect;
	float t =(  ( ( Center.x - From.x ) * Delta.x ) +
				( ( Center.y - From.y ) * Delta.y )) /
				Delta.MagSqr() ;
 
    if( t < 0.0f)	// before from
	{
		Intersect = From;
	} else
    if( t > 1.0f)	// before from
	{
		Intersect = To;
	} else
	{
		// closes point is somewhere along the ray. Is it inside?
		Intersect.x = From.x + t * ( Delta.x );
		Intersect.y = From.y + t * ( Delta.y );
	}

	math::Vec2	Distance = Center-Intersect;

	// find distance from point to line. Do an early out without a sqrt?
	float Dist = Distance.Magnitude();
	if (Dist > Radius+LineRad)  return false; //no collision. 
	// collided
	Penetration = (Radius+LineRad)-Dist;
	//find normal
//	Normal = Distance / Dist;
	Normal = Distance;
	Normal .Normalize();

	return true;
}	

bool	Intersection::AABBCapsule2D(const math::Vec2& Min, const math::Vec2& Max, const math::Vec2& From, const math::Vec2& To, float LineRad, float& Penetration, math::Vec2& Normal)
{
	// TODO: fill this one in; it'll come in handy later
	assert("TODO" && 0);
	return false;
}


bool	Intersection::CapsuleCapsule2D(const math::Vec2& FromA, const math::Vec2& ToA, const math::Vec2& FromB, const math::Vec2& ToB, float LineRadA, float LineRadB)
{
	math::Vec2	PA, PB;
	if (ClosestDistanceBetweenSegments2D(FromA, ToA, FromB, ToB, PA, PB) < LineRadA + LineRadB)
	{
		// inside. Do some verifications as well?
		return true;			
	}
	return false;
}

bool Intersection::RayPoint(const Vec2& Point, const Vec2& Origin, const Vec2& Line, Vec2& Nearest)
{
	Vec2 tmp;
	float t;

	tmp = Point - Origin;
	t = (Line ^ tmp) / (Line ^ Line);

	if ((t < 0.0f) || (t > 1.0f)) return false;

	Nearest = Origin + (t* Line);
	return true;
}

bool	Intersection::RayAABB2D(const Vec2&	vMin,	const Vec2& vMax, const Vec2& _vFrom, const Vec2& _vTo, Vec2& _vIntersection)
{
	math::Vec3	Inter;
	// hack.. just redo it as a 3d operation.. should really rewrite it
	bool	Res = RayAABB(
		math::Vec3(vMin.x, vMin.y, -1), 
		math::Vec3(vMax.x, vMax.y, 1), 
		math::Vec3(_vFrom.x, _vFrom.y, 0),
		math::Vec3(_vTo.x, _vTo.y, 0), Inter);
	if (Res)
	{
		_vIntersection.x = Inter.x;
		_vIntersection.y = Inter.y;
		return true;
	} else
		return false;
}

math::Vec2		Intersection::ClosestPointOnLine2D(const math::Vec2& A, const math::Vec2& B, const math::Vec2& P, bool segmentClamp)
{
    math::Vec2 AP = P - A;
    math::Vec2 AB = B - A;
    float ab2 = AB.x*AB.x + AB.y*AB.y;
    float ap_ab = AP.x*AB.x + AP.y*AB.y;
    float t = ap_ab / ab2;
    if (segmentClamp)
    {
         if (t < 0.0f) t = 0.0f;
         else if (t > 1.0f) t = 1.0f;
    }
    math::Vec2 Closest = A + AB * t;
    return Closest;
}

float			Intersection::DistanceToLine2D(const math::Vec2& A, const math::Vec2& B, const math::Vec2& P)
{
	math::Vec2	LineP = ClosestPointOnLine2D(A, B, P, true);
	return (LineP - P).Magnitude();
}

float	Intersection::ClosestDistanceBetweenSegments2D(const math::Vec2& p1, const math::Vec2& p2, const math::Vec2& p3, const math::Vec2& p4, math::Vec2& PA, math::Vec2& PB)
{
	math::Vec2  u = p2 - p1;
    math::Vec2	v = p4 - p3;
    math::Vec2	w = p1 - p3;
	float    a = u.DotProd(u);        // always >= 0
    float    b = u.DotProd(v);
    float    c = v.DotProd(v);        // always >= 0
    float    d = u.DotProd(w);
    float    e = v.DotProd(w);
    float    D = a*c - b*b;       // always >= 0
  float    sc, sN, sD = D;      // sc = sN / sD, default sD = D >= 0
    float    tc, tN, tD = D;      // tc = tN / tD, default tD = D >= 0

	static const float	SMALL_NUM = 0.00000001f;
    // compute the line parameters of the two closest points

    // compute the line parameters of the two closest points
    if (D < SMALL_NUM) 
	{					// the lines are almost parallel
        sN = 0.0;        // force using point P0 on segment S1
        sD = 1.0;        // to prevent possible division by 0.0 later
        tN = e;
        tD = c;
    }
    else 
	{                // get the closest points on the infinite lines
        sN = (b*e - c*d);
        tN = (a*e - b*d);
        if (sN < 0.0) 
		{       // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
        } else 
		if (sN > sD) 
		{  // sc > 1 => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0) 
	{           // tc < 0 => the t=0 edge is visible
        tN = 0.0;
        // recompute sc for this edge
        if (-d < 0.0)
		{
            sN = 0.0;
		} else 
		if (-d > a)
		{
            sN = sD;
		}
        else 
		{
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD) 
	{      // tc > 1 => the t=1 edge is visible
        tN = tD;
        // recompute sc for this edge
        if ((-d + b) < 0.0)
		{
            sN = 0;
		} else 
		if ((-d + b) > a)
		{
            sN = sD;
		} else 
		{
            sN = (-d + b);
            sD = a;
        }
    }
    // finally do the division to get sc and tc
    sc = (fabs(sN) < SMALL_NUM ? 0.0f : sN / sD);
    tc = (fabs(tN) < SMALL_NUM ? 0.0f : tN / tD);

    // get the difference of the two closest points
	PA = p1 + (u *sc);
	PB = p3 + (v *tc);
	return (PA-PB).Magnitude();
//    Vector   dP = w + (sc * u) - (tc * v);  // = S1(sc) - S2(tc)
//    return norm(dP);   // return the closest distance
}


bool Intersection::PointInsidePoly2D(const Vec2& v, Vec2* PointA, int PointC)
{
	assert( (PointC > 2) && "Poly must be at least triangle or more!!!" );

	// algorithm: http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
	bool IsInside = false;
	int i, j;

	for (i = 0, j = (PointC-1); i < PointC; j = i++)
	{
		if ( ((PointA[i].y > v.y) != (PointA[j].y > v.y)) &&
			 (v.x < (PointA[j].x-PointA[i].x) * (v.y-PointA[i].y) / (PointA[j].y-PointA[i].y) + PointA[i].x) )
		{
			IsInside = !IsInside;
		}
	}

	return IsInside;
}

bool Intersection::PointInsideRect2D(const Vec2& Point, const Vec2& Pos, const Vec2& Size, float Rot, bool IsCentered)
{
	return PointInsideRect2DFast(Point, Pos,Size,IsCentered);
	// WARNING: this function is untested; use at your own risk
	// however (DirTL), (PointA math for IsCentered) and (PointInsidePoly2D) works -- just need to verify Dir is correct

	/* rect class is borked
	Rect r(Pos, Size);
	Vec2 PointA[4];

	if (IsCentered)
		r.GetCenteredRotatedCoords(Rot, PointA);
	else
		r.GetRotatedCoords(Rot, PointA);
	*/

	Vec2 PointA[4];
	Vec2 Dir, DirTL;
	Facing f;

	f.SetCurrent(Rot);
	Dir = f.GetForward(); // is this correct? is Dir normalized?
	DirTL.x = -Dir.y;
	DirTL.y = Dir.x;

	if (IsCentered)
	{
		PointA[0]	= Pos - (0.5f * Size.x * DirTL) - (0.5f * Size.y * Dir);
		PointA[1]	= PointA[0] - (0.5f * Size.y * Dir);
		PointA[2]	= PointA[1] + (0.5f * Size.x * DirTL);
		PointA[3]	= PointA[2] + (0.5f * Size.y * Dir);
	}
	else
	{
		// this branch not tested; however unless i'm an idiot it should work
		PointA[0]	= Pos;
		PointA[1]	= PointA[0] + (Size.y * Dir);
		PointA[2]	= PointA[1] - (Size.x * DirTL);
		PointA[3]	= PointA[2] - (Size.y * Dir);
	}

	return PointInsidePoly2D(Point, PointA, 4);
}

bool	Intersection::PointInsideRect2DFast(const Vec2& Point, const Vec2& Pos, const Vec2& Size, bool IsCentered)
{
	if(IsCentered)
	{
		math::Vec2 HalfSize = Size * 0.5f;
		if( Point.x > Pos.x + HalfSize.x  )
			return false;

		if( Point.x < Pos.x - HalfSize.x  )
			return false;

		if( Point.y > Pos.y + HalfSize.y  )
			return false;

		if( Point.y < Pos.y - HalfSize.y  )
			return false;

	}
	else
	{
		if( Point.x > Pos.x + Size.x)
			return false;

		if( Point.x < Pos.x)
			return false;

		if( Point.y > Pos.y + Size.y)
			return false;

		if( Point.y < Pos.y)
			return false;
	}

	return true;
}
